#include "ACMMP.h"

#define mul4(v,k) { \
    v->x = v->x * k; \
    v->y = v->y * k; \
    v->z = v->z * k;\
}

#define vecdiv4(v,k) { \
    v->x = v->x / k; \
    v->y = v->y / k; \
    v->z = v->z / k;\
}

__device__  void sort_small(float *d, const int n)
{
    int j;
    for (int i = 1; i < n; i++) {
        float tmp = d[i];
        for (j = i; j >= 1 && tmp < d[j-1]; j--)
            d[j] = d[j-1];
        d[j] = tmp;
    }
}

//没用上
__device__ void sort_small_weighted(float *d, float *w, int n)
{
    int j;
    for (int i = 1; i < n; i++) {
        float tmp = d[i];
        float tmp_w = w[i];
        for (j = i; j >= 1 && tmp < d[j - 1]; j--) {
            d[j] = d[j - 1];
            w[j] = w[j - 1];
        }
        d[j] = tmp;
        w[j] = tmp_w;
    }
}

//找最小代价邻域像素索引
__device__ int FindMinCostIndex(const float *costs, const int n)
{
    float min_cost = costs[0];
    int min_cost_idx = 0;
    for (int idx = 1; idx < n; ++idx) {
        if (costs[idx] <= min_cost) {
            min_cost = costs[idx];
            min_cost_idx = idx;
        }
    }
    return min_cost_idx;
}

//找最大代价邻域像素索引
__device__ int FindMaxCostIndex(const float *costs, const int n)
{
    float max_cost = costs[0];
    int max_cost_idx = 0;
    for (int idx = 1; idx < n; ++idx) {
        if (costs[idx] >= max_cost) {
            max_cost = costs[idx];
            max_cost_idx = idx;
        }
    }
    return max_cost_idx;
}

__device__  void setBit(unsigned int &input, const unsigned int n)
{
    input |= (unsigned int)(1 << n);
}

__device__  int isSet(unsigned int input, const unsigned int n)
{
    return (input >> n) & 1;
}

__device__ void Mat33DotVec3(const float mat[9], const float4 vec, float4 *result)
{
  result->x = mat[0] * vec.x + mat[1] * vec.y + mat[2] * vec.z;
  result->y = mat[3] * vec.x + mat[4] * vec.y + mat[5] * vec.z;
  result->z = mat[6] * vec.x + mat[7] * vec.y + mat[8] * vec.z;
}


//向量叉乘
__device__ void Vec3CrossVec3(const float4 vec1, const float4 vec2, float4 *result)
{
    result->x = vec1.y * vec2.z - vec1.z * vec2.y;
    result->y = vec1.z * vec2.x - vec1.x * vec2.z;
    result->z = vec1.x * vec2.y - vec1.y * vec2.x;
}

//向量内积
__device__ float Vec3DotVec3(const float4 vec1, const float4 vec2)
{
    return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
}

//归一化向量
__device__ void NormalizeVec3 (float4 *vec)
{
    const float normSquared = vec->x * vec->x + vec->y * vec->y + vec->z * vec->z;
    const float inverse_sqrt = rsqrtf (normSquared);    // 表示 1/sqrt(normSquared)     (float)
    vec->x *= inverse_sqrt;
    vec->y *= inverse_sqrt;
    vec->z *= inverse_sqrt;
}

//概率密度函数pdf转为累积分布函数cdf
__device__ void TransformPDFToCDF(float* probs, const int num_probs)
{
    float prob_sum = 0.0f;
    for (int i = 0; i < num_probs; ++i) {
        prob_sum += probs[i];
    }
    const float inv_prob_sum = 1.0f / prob_sum;

    float cum_prob = 0.0f;
    for (int i = 0; i < num_probs; ++i) {
        const float prob = probs[i] * inv_prob_sum;
        cum_prob += prob;
        probs[i] = cum_prob;
    }
}

__device__ void Get3DPoint(const Camera camera, const int2 p, const float depth, float *X)
{
    X[0] = depth * (p.x - camera.K[2]) / camera.K[0];
    X[1] = depth * (p.y - camera.K[5]) / camera.K[4];
    X[2] = depth;
}

__device__ float4 GetViewDirection(const Camera camera, const int2 p, const float depth)
{
    float X[3];
    Get3DPoint(camera, p, depth, X);
    float norm = sqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2]);

    float4 view_direction;
    view_direction.x = X[0] / norm;
    view_direction.y = X[1] / norm;
    view_direction.z =  X[2] / norm;
    view_direction.w = 0;
    return view_direction;
}

//根据平面(法向量)和相机坐标计算点到平面距离
__device__ float GetDistance2Origin(const Camera camera, const int2 p, const float depth, const float4 normal)
{
    float X[3];
    Get3DPoint(camera, p, depth, X);
    return -(normal.x * X[0] + normal.y * X[1] + normal.z * X[2]);
}

__device__   float SpatialGauss(float x1, float y1, float x2, float y2, float sigma, float mu = 0.0)
{
    float dis = pow(x1 - x2, 2) + pow(y1 - y2, 2) - mu;
    return exp(-1.0 * dis / (2 * sigma * sigma));
}

__device__  float RangeGauss(float x, float sigma, float mu = 0.0)
{
    float x_p = x - mu;
    return exp(-1.0 * (x_p * x_p) / (2 * sigma * sigma));
}

__device__ float ComputeDepthfromPlaneHypothesis(const Camera camera, const float4 plane_hypothesis, const int2 p)
{
    return -plane_hypothesis.w * camera.K[0] / ((p.x - camera.K[2]) * plane_hypothesis.x + (camera.K[0] / camera.K[4]) * (p.y - camera.K[5]) * plane_hypothesis.y + camera.K[0] * plane_hypothesis.z);
}

//产生随机法向量
__device__ float4 GenerateRandomNormal(const Camera camera, const int2 p, curandState *rand_state, const float depth)
{
    float4 normal;
    float q1 = 1.0f;
    float q2 = 1.0f;
    float s = 2.0f;
    while (s >= 1.0f) {
        q1 = 2.0f * curand_uniform(rand_state) -1.0f;
        q2 = 2.0f * curand_uniform(rand_state) - 1.0f;
        s = q1 * q1 + q2 * q2;
    }
    const float sq = sqrt(1.0f - s);
    normal.x = 2.0f * q1 * sq;
    normal.y = 2.0f * q2 * sq;
    normal.z = 1.0f - 2.0f * s;
    normal.w = 0;

    float4 view_direction = GetViewDirection(camera, p, depth);
    float dot_product = normal.x * view_direction.x + normal.y * view_direction.y + normal.z * view_direction.z;
    if (dot_product > 0.0f) {
        normal.x = -normal.x;
        normal.y = -normal.y;
        normal.z = - normal.z;
    }
    NormalizeVec3(&normal);
    return normal;
}
//产生扰动法向量
__device__ float4 GeneratePerturbedNormal(const Camera camera, const int2 p, const float4 normal, curandState *rand_state, const float perturbation)
{
    float4 view_direction = GetViewDirection(camera, p, 1.0f);

    const float a1 = (curand_uniform(rand_state) - 0.5f) * perturbation;
    const float a2 = (curand_uniform(rand_state) - 0.5f) * perturbation;
    const float a3 = (curand_uniform(rand_state) - 0.5f) * perturbation;

    const float sin_a1 = sin(a1);
    const float sin_a2 = sin(a2);
    const float sin_a3 = sin(a3);
    const float cos_a1 = cos(a1);
    const float cos_a2 = cos(a2);
    const float cos_a3 = cos(a3);

    float R[9];
    R[0] = cos_a2 * cos_a3;
    R[1] = cos_a3 * sin_a1 * sin_a2 - cos_a1 * sin_a3;
    R[2] = sin_a1 * sin_a3 + cos_a1 * cos_a3 * sin_a2;
    R[3] = cos_a2 * sin_a3;
    R[4] = cos_a1 * cos_a3 + sin_a1 * sin_a2 * sin_a3;
    R[5] = cos_a1 * sin_a2 * sin_a3 - cos_a3 * sin_a1;
    R[6] = -sin_a2;
    R[7] = cos_a2 * sin_a1;
    R[8] = cos_a1 * cos_a2;

    float4 normal_perturbed;
    Mat33DotVec3(R, normal, &normal_perturbed);

    if (Vec3DotVec3(normal_perturbed, view_direction) >= 0.0f) {
        normal_perturbed = normal;
    }

    NormalizeVec3(&normal_perturbed);
    return normal_perturbed;
}

//产生随机平面
__device__ float4 GenerateRandomPlaneHypothesis(const Camera camera, const int2 p, curandState *rand_state, const float depth_min, const float depth_max)
{
    float depth = curand_uniform(rand_state) * (depth_max - depth_min) + depth_min;
    float4 plane_hypothesis = GenerateRandomNormal(camera, p, rand_state, depth);
    plane_hypothesis.w = GetDistance2Origin(camera, p, depth, plane_hypothesis);
    return plane_hypothesis;
}

//产生扰动平面
__device__ float4 GeneratePertubedPlaneHypothesis(const Camera camera, const int2 p, curandState *rand_state, const float perturbation, const float4 plane_hypothesis_now, const float depth_now, const float depth_min, const float depth_max)
{
    float depth_perturbed = depth_now;

    float dist_perturbed = plane_hypothesis_now.w;
    const float dist_min_perturbed = (1 - perturbation) * dist_perturbed;
    const float dist_max_perturbed = (1 + perturbation) * dist_perturbed;
    float4 plane_hypothesis_temp = plane_hypothesis_now;
    do {
        dist_perturbed = curand_uniform(rand_state) * (dist_max_perturbed - dist_min_perturbed) + dist_min_perturbed;
        plane_hypothesis_temp.w = dist_perturbed;
        depth_perturbed = ComputeDepthfromPlaneHypothesis(camera, plane_hypothesis_temp, p);
    } while (depth_perturbed < depth_min && depth_perturbed > depth_max);

    float4 plane_hypothesis = GeneratePerturbedNormal(camera, p, plane_hypothesis_now, rand_state, perturbation * M_PI);
    plane_hypothesis.w = dist_perturbed;
    return plane_hypothesis;
}

//计算单应矩阵
__device__ void ComputeHomography(const Camera ref_camera, const Camera src_camera, const float4 plane_hypothesis, float *H)
{
    float ref_C[3];
    float src_C[3];
    ref_C[0] = -(ref_camera.R[0] * ref_camera.t[0] + ref_camera.R[3] * ref_camera.t[1] + ref_camera.R[6] * ref_camera.t[2]);
    ref_C[1] = -(ref_camera.R[1] * ref_camera.t[0] + ref_camera.R[4] * ref_camera.t[1] + ref_camera.R[7] * ref_camera.t[2]);
    ref_C[2] = -(ref_camera.R[2] * ref_camera.t[0] + ref_camera.R[5] * ref_camera.t[1] + ref_camera.R[8] * ref_camera.t[2]);
    src_C[0] = -(src_camera.R[0] * src_camera.t[0] + src_camera.R[3] * src_camera.t[1] + src_camera.R[6] * src_camera.t[2]);
    src_C[1] = -(src_camera.R[1] * src_camera.t[0] + src_camera.R[4] * src_camera.t[1] + src_camera.R[7] * src_camera.t[2]);
    src_C[2] = -(src_camera.R[2] * src_camera.t[0] + src_camera.R[5] * src_camera.t[1] + src_camera.R[8] * src_camera.t[2]);

    float R_relative[9];
    float C_relative[3];
    float t_relative[3];
    R_relative[0] = src_camera.R[0] * ref_camera.R[0] + src_camera.R[1] * ref_camera.R[1] + src_camera.R[2] *ref_camera.R[2];
    R_relative[1] = src_camera.R[0] * ref_camera.R[3] + src_camera.R[1] * ref_camera.R[4] + src_camera.R[2] *ref_camera.R[5];
    R_relative[2] = src_camera.R[0] * ref_camera.R[6] + src_camera.R[1] * ref_camera.R[7] + src_camera.R[2] *ref_camera.R[8];
    R_relative[3] = src_camera.R[3] * ref_camera.R[0] + src_camera.R[4] * ref_camera.R[1] + src_camera.R[5] *ref_camera.R[2];
    R_relative[4] = src_camera.R[3] * ref_camera.R[3] + src_camera.R[4] * ref_camera.R[4] + src_camera.R[5] *ref_camera.R[5];
    R_relative[5] = src_camera.R[3] * ref_camera.R[6] + src_camera.R[4] * ref_camera.R[7] + src_camera.R[5] *ref_camera.R[8];
    R_relative[6] = src_camera.R[6] * ref_camera.R[0] + src_camera.R[7] * ref_camera.R[1] + src_camera.R[8] *ref_camera.R[2];
    R_relative[7] = src_camera.R[6] * ref_camera.R[3] + src_camera.R[7] * ref_camera.R[4] + src_camera.R[8] *ref_camera.R[5];
    R_relative[8] = src_camera.R[6] * ref_camera.R[6] + src_camera.R[7] * ref_camera.R[7] + src_camera.R[8] *ref_camera.R[8];
    C_relative[0] = (ref_C[0] - src_C[0]);
    C_relative[1] = (ref_C[1] - src_C[1]);
    C_relative[2] = (ref_C[2] - src_C[2]);
    t_relative[0] = src_camera.R[0] * C_relative[0] + src_camera.R[1] * C_relative[1] + src_camera.R[2] * C_relative[2];
    t_relative[1] = src_camera.R[3] * C_relative[0] + src_camera.R[4] * C_relative[1] + src_camera.R[5] * C_relative[2];
    t_relative[2] = src_camera.R[6] * C_relative[0] + src_camera.R[7] * C_relative[1] + src_camera.R[8] * C_relative[2];

    H[0] = R_relative[0] - t_relative[0] * plane_hypothesis.x / plane_hypothesis.w;
    H[1] = R_relative[1] - t_relative[0] * plane_hypothesis.y / plane_hypothesis.w;
    H[2] = R_relative[2] - t_relative[0] * plane_hypothesis.z / plane_hypothesis.w;
    H[3] = R_relative[3] - t_relative[1] * plane_hypothesis.x / plane_hypothesis.w;
    H[4] = R_relative[4] - t_relative[1] * plane_hypothesis.y / plane_hypothesis.w;
    H[5] = R_relative[5] - t_relative[1] * plane_hypothesis.z / plane_hypothesis.w;
    H[6] = R_relative[6] - t_relative[2] * plane_hypothesis.x / plane_hypothesis.w;
    H[7] = R_relative[7] - t_relative[2] * plane_hypothesis.y / plane_hypothesis.w;
    H[8] = R_relative[8] - t_relative[2] * plane_hypothesis.z / plane_hypothesis.w;

    float tmp[9];
    tmp[0] = H[0] / ref_camera.K[0];
    tmp[1] = H[1] / ref_camera.K[4];
    tmp[2] = -H[0] * ref_camera.K[2] / ref_camera.K[0] - H[1] * ref_camera.K[5] / ref_camera.K[4] + H[2];
    tmp[3] = H[3] / ref_camera.K[0];
    tmp[4] = H[4] / ref_camera.K[4];
    tmp[5] = -H[3] * ref_camera.K[2] / ref_camera.K[0] - H[4] * ref_camera.K[5] / ref_camera.K[4] + H[5];
    tmp[6] = H[6] / ref_camera.K[0];
    tmp[7] = H[7] / ref_camera.K[4];
    tmp[8] = -H[6] * ref_camera.K[2] / ref_camera.K[0] - H[7] * ref_camera.K[5] / ref_camera.K[4] + H[8];

    H[0] = src_camera.K[0] * tmp[0] + src_camera.K[2] * tmp[6];
    H[1] = src_camera.K[0] * tmp[1] + src_camera.K[2] * tmp[7];
    H[2] = src_camera.K[0] * tmp[2] + src_camera.K[2] * tmp[8];
    H[3] = src_camera.K[4] * tmp[3] + src_camera.K[5] * tmp[6];
    H[4] = src_camera.K[4] * tmp[4] + src_camera.K[5] * tmp[7];
    H[5] = src_camera.K[4] * tmp[5] + src_camera.K[5] * tmp[8];
    H[6] = src_camera.K[8] * tmp[6];
    H[7] = src_camera.K[8] * tmp[7];
    H[8] = src_camera.K[8] * tmp[8];
}

//通过单应矩阵计算投影点
__device__ float2 ComputeCorrespondingPoint(const float *H, const int2 p)
{
    float3 pt;
    pt.x = H[0] * p.x + H[1] * p.y + H[2];
    pt.y = H[3] * p.x + H[4] * p.y + H[5];
    pt.z = H[6] * p.x + H[7] * p.y + H[8];
    return make_float2(pt.x / pt.z, pt.y / pt.z);
}


__device__ float4 TransformNormal(const Camera camera, float4 plane_hypothesis)
{
    float4 transformed_normal;
    transformed_normal.x = camera.R[0] * plane_hypothesis.x + camera.R[3] * plane_hypothesis.y + camera.R[6] * plane_hypothesis.z;
    transformed_normal.y = camera.R[1] * plane_hypothesis.x + camera.R[4] * plane_hypothesis.y + camera.R[7] * plane_hypothesis.z;
    transformed_normal.z = camera.R[2] * plane_hypothesis.x + camera.R[5] * plane_hypothesis.y + camera.R[8] * plane_hypothesis.z;
    transformed_normal.w = plane_hypothesis.w;
    return transformed_normal;
}

__device__ float4 TransformNormal2RefCam(const Camera camera, float4 plane_hypothesis)
{
    float4 transformed_normal;
    transformed_normal.x = camera.R[0] * plane_hypothesis.x + camera.R[1] * plane_hypothesis.y + camera.R[2] * plane_hypothesis.z;
    transformed_normal.y = camera.R[3] * plane_hypothesis.x + camera.R[4] * plane_hypothesis.y + camera.R[5] * plane_hypothesis.z;
    transformed_normal.z = camera.R[6] * plane_hypothesis.x + camera.R[7] * plane_hypothesis.y + camera.R[8] * plane_hypothesis.z;
    transformed_normal.w = plane_hypothesis.w;
    return transformed_normal;
}

//计算双边权重
__device__ float ComputeBilateralWeight(const float x_dist, const float y_dist, const float pix, const float center_pix, const float sigma_spatial, const float sigma_color)
{
    const float spatial_dist = sqrt(x_dist * x_dist + y_dist * y_dist);
    const float color_dist = fabs(pix - center_pix);
    return exp(-spatial_dist / (2.0f * sigma_spatial* sigma_spatial) - color_dist / (2.0f * sigma_color * sigma_color));
}

//计算双边权重ncc
__device__ float ComputeBilateralNCC(const cudaTextureObject_t ref_image, const Camera ref_camera, const cudaTextureObject_t src_image, const Camera src_camera, const int2 p, const float4 plane_hypothesis, const PatchMatchParams params)
{
    const float cost_max = 2.0f;
    int radius = params.patch_size / 2;

    float H[9];
    ComputeHomography(ref_camera, src_camera, plane_hypothesis, H);
    float2 pt = ComputeCorrespondingPoint(H, p);
    if (pt.x >= src_camera.width || pt.x < 0.0f || pt.y >= src_camera.height || pt.y < 0.0f) {
        return cost_max;
    }

    float cost = 0.0f;
    {
        float sum_ref = 0.0f;
        float sum_ref_ref = 0.0f;
        float sum_src = 0.0f;
        float sum_src_src = 0.0f;
        float sum_ref_src = 0.0f;
        float bilateral_weight_sum = 0.0f;
        const float ref_center_pix = tex2D<float>(ref_image, p.x + 0.5f, p.y + 0.5f);

        for (int i = -radius; i < radius + 1; i += params.radius_increment) {
            float sum_ref_row = 0.0f;
            float sum_src_row = 0.0f;
            float sum_ref_ref_row = 0.0f;
            float sum_src_src_row = 0.0f;
            float sum_ref_src_row = 0.0f;
            float bilateral_weight_sum_row = 0.0f;

            for (int j = -radius; j < radius + 1; j += params.radius_increment) {
                const int2 ref_pt = make_int2(p.x + i, p.y + j);
                const float ref_pix = tex2D<float>(ref_image, ref_pt.x + 0.5f, ref_pt.y + 0.5f);
                float2 src_pt = ComputeCorrespondingPoint(H, ref_pt);
                const float src_pix = tex2D<float>(src_image, src_pt.x + 0.5f, src_pt.y + 0.5f);

                float weight = ComputeBilateralWeight(i, j, ref_pix, ref_center_pix, params.sigma_spatial, params.sigma_color);

                sum_ref_row += weight * ref_pix;
                sum_ref_ref_row += weight * ref_pix * ref_pix;
                sum_src_row += weight * src_pix;
                sum_src_src_row += weight * src_pix * src_pix;
                sum_ref_src_row += weight * ref_pix * src_pix;
                bilateral_weight_sum_row += weight;
            }

            sum_ref += sum_ref_row;
            sum_ref_ref += sum_ref_ref_row;
            sum_src += sum_src_row;
            sum_src_src += sum_src_src_row;
            sum_ref_src += sum_ref_src_row;
            bilateral_weight_sum += bilateral_weight_sum_row;
        }
        const float inv_bilateral_weight_sum = 1.0f / bilateral_weight_sum;
        sum_ref *= inv_bilateral_weight_sum;
        sum_ref_ref *= inv_bilateral_weight_sum;
        sum_src *= inv_bilateral_weight_sum;
        sum_src_src *= inv_bilateral_weight_sum;
        sum_ref_src *= inv_bilateral_weight_sum;

        const float var_ref = sum_ref_ref - sum_ref * sum_ref;
        const float var_src = sum_src_src - sum_src * sum_src;

        const float kMinVar = 1e-5f;
        if (var_ref < kMinVar || var_src < kMinVar) {
            return cost = cost_max;
        } else {
            const float covar_src_ref = sum_ref_src - sum_ref * sum_src;
            const float var_ref_src = sqrt(var_ref * var_src);
            return cost = max(0.0f, min(cost_max, 1.0f - covar_src_ref / var_ref_src));
        }
    }
}

__device__ float ComputeMultiViewInitialCostandSelectedViews(const cudaTextureObject_t *images, const Camera *cameras, const int2 p, const float4 plane_hypothesis, unsigned int *selected_views, const PatchMatchParams params)
{
    float cost_max = 2.0f;
    float cost_vector[32] = {2.0f};
    float cost_vector_copy[32] = {2.0f};
    int cost_count = 0;
    int num_valid_views = 0;

    for (int i = 1; i < params.num_images; ++i) {
        float c = ComputeBilateralNCC(images[0], cameras[0], images[i], cameras[i], p, plane_hypothesis, params);
        cost_vector[i - 1] = c;
        cost_vector_copy[i - 1] = c;
        cost_count++;
        if (c < cost_max) {
            num_valid_views++;
        }
    }

    sort_small(cost_vector, cost_count);
    *selected_views = 0;

    int top_k = min(num_valid_views, params.top_k);
    if (top_k > 0) {
        float cost = 0.0f;
        for (int i = 0; i < top_k; ++i) {
            cost += cost_vector[i];
        }
        float cost_threshold = cost_vector[top_k - 1];
        for (int i = 0; i < params.num_images - 1; ++i) {
            if (cost_vector_copy[i] <= cost_threshold) {
                setBit(*selected_views, i);
            }
        }
        return cost / top_k;
    } else {
        return cost_max;
    }
}

__device__ void ComputeMultiViewCostVector(const cudaTextureObject_t *images, const Camera *cameras, const int2 p, const float4 plane_hypothesis, float *cost_vector, const PatchMatchParams params)
{
    for (int i = 1; i < params.num_images; ++i) {
        cost_vector[i - 1] = ComputeBilateralNCC(images[0], cameras[0], images[i], cameras[i], p, plane_hypothesis, params);
    }
}

__device__ float3 Get3DPointonRefCam_cu(const int x, const int y, const float depth, const Camera camera)
{
    float3 pointX;
    // Reprojection
    pointX.x = depth * (x - camera.K[2]) / camera.K[0];
    pointX.y = depth * (y - camera.K[5]) / camera.K[4];
    pointX.z = depth;

    return pointX;
}



//世界坐标转换
__device__ float3 Get3DPointonWorld_cu(const float x, const float y, const float depth, const Camera camera)
{
    float3 pointX;
    float3 tmpX;
    // Reprojection
    pointX.x = depth * (x - camera.K[2]) / camera.K[0];
    pointX.y = depth * (y - camera.K[5]) / camera.K[4];
    pointX.z = depth;

    // Rotation
    tmpX.x = camera.R[0] * pointX.x + camera.R[3] * pointX.y + camera.R[6] * pointX.z;
    tmpX.y = camera.R[1] * pointX.x + camera.R[4] * pointX.y + camera.R[7] * pointX.z;
    tmpX.z = camera.R[2] * pointX.x + camera.R[5] * pointX.y + camera.R[8] * pointX.z;

    // Transformation
    float3 C;
    C.x = -(camera.R[0] * camera.t[0] + camera.R[3] * camera.t[1] + camera.R[6] * camera.t[2]);
    C.y = -(camera.R[1] * camera.t[0] + camera.R[4] * camera.t[1] + camera.R[7] * camera.t[2]);
    C.z = -(camera.R[2] * camera.t[0] + camera.R[5] * camera.t[1] + camera.R[8] * camera.t[2]);
    pointX.x = tmpX.x + C.x;
    pointX.y = tmpX.y + C.y;
    pointX.z = tmpX.z + C.z;

    return pointX;
}

//投影点计算
__device__ void ProjectonCamera_cu(const float3 PointX, const Camera camera, float2 &point, float &depth)
{
    float3 tmp;
    tmp.x = camera.R[0] * PointX.x + camera.R[1] * PointX.y + camera.R[2] * PointX.z + camera.t[0];
    tmp.y = camera.R[3] * PointX.x + camera.R[4] * PointX.y + camera.R[5] * PointX.z + camera.t[1];
    tmp.z = camera.R[6] * PointX.x + camera.R[7] * PointX.y + camera.R[8] * PointX.z + camera.t[2];

    depth = camera.K[6] * tmp.x + camera.K[7] * tmp.y + camera.K[8] * tmp.z;
    point.x = (camera.K[0] * tmp.x + camera.K[1] * tmp.y + camera.K[2] * tmp.z) / depth;
    point.y = (camera.K[3] * tmp.x + camera.K[4] * tmp.y + camera.K[5] * tmp.z) / depth;
}

//计算几何一致性
__device__ float ComputeGeomConsistencyCost(const cudaTextureObject_t depth_image, const Camera ref_camera, const Camera src_camera, const float4 plane_hypothesis, const int2 p)
{
    const float max_cost = 3.0f;

    float depth = ComputeDepthfromPlaneHypothesis(ref_camera, plane_hypothesis, p);
    float3 forward_point = Get3DPointonWorld_cu(p.x, p.y, depth, ref_camera);

    float2 src_pt;
    float src_d;
    ProjectonCamera_cu(forward_point, src_camera, src_pt, src_d);
    const float src_depth = tex2D<float>(depth_image,  (int)src_pt.x + 0.5f, (int)src_pt.y + 0.5f);

    if (src_depth == 0.0f) {
        return max_cost;
    }

    float3 src_3D_pt = Get3DPointonWorld_cu(src_pt.x, src_pt.y, src_depth, src_camera);

    float2 backward_point;
    float ref_d;
    ProjectonCamera_cu(src_3D_pt, ref_camera, backward_point, ref_d);

    const float diff_col = p.x - backward_point.x;
    const float diff_row = p.y - backward_point.y;
    return min(max_cost, sqrt(diff_col * diff_col + diff_row * diff_row));
}

__global__ void RandomInitialization(cudaTextureObjects *texture_objects, Camera *cameras, float4 *plane_hypotheses,  float4 *scaled_plane_hypotheses, float *costs,  float *pre_costs,  curandState *rand_states, unsigned int *selected_views, float4 *prior_planes, unsigned int *plane_masks, const PatchMatchParams params)
{
    const int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    int width = cameras[0].width;
    int height = cameras[0].height;

    if (p.x >= width || p.y >= height) {
        return;
    }

    const int center = p.y * width + p.x;
    curand_init(clock64(), p.y, p.x, &rand_states[center]);

    if (!params.geom_consistency && !params.hierarchy ) {
        plane_hypotheses[center] = GenerateRandomPlaneHypothesis(cameras[0], p, &rand_states[center], params.depth_min, params.depth_max);
        costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
    }
    else if (params.planar_prior) {
        if (plane_masks[center] > 0 && costs[center] >= 0.1f) {
            float perturbation = 0.02f;

            float4 plane_hypothesis = prior_planes[center];
            float depth_perturbed = plane_hypothesis.w;
            const float depth_min_perturbed = (1 - 3 * perturbation) * depth_perturbed;
            const float depth_max_perturbed = (1 + 3 * perturbation) * depth_perturbed;
            depth_perturbed = curand_uniform(&rand_states[center]) * (depth_max_perturbed - depth_min_perturbed) + depth_min_perturbed;
            float4 plane_hypothesis_perturbed = GeneratePerturbedNormal(cameras[0], p, plane_hypothesis, &rand_states[center], 3 * perturbation * M_PI);
            plane_hypothesis_perturbed.w = depth_perturbed;
            plane_hypotheses[center] = plane_hypothesis_perturbed;
            costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
        }
        else {
            float4 plane_hypothesis = plane_hypotheses[center];
            float depth = plane_hypothesis.w;
            plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depth, plane_hypothesis);
            plane_hypotheses[center] = plane_hypothesis;
            costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
        }
    }
    else {
        if(params.upsample) {
            const float scale = 1.0 * params.scaled_cols / width;
            const float sigmad = 0.50;
            const float sigmar = 25.5;
            const int Imagescale = max(width / params.scaled_cols , height / params.scaled_rows);
            const int WinWidth =Imagescale * Imagescale + 1;
            int num_neighbors = WinWidth / 2;

            const float o_y = p.y * scale;
            const float o_x = p.x * scale;
            const float refPix = tex2D<float>(texture_objects[0].images[0], p.x + 0.5f, p.y + 0.5f);
            int r_y = 0;
            int r_ys = 0;
            int r_x = 0;
            int r_xs = 0;
            float sgauss = 0.0, rgauss = 0.0, totalgauss = 0.0;
            float c_total_val = 0.0, normalizing_factor = 0.0;
            float  srcPix = 0, neighborPix = 0;
            float4 srcNorm;
            float4 n_total_val;
            n_total_val.x = 0; n_total_val.y = 0; n_total_val.z = 0; n_total_val.w = 0;
     \
            for (int j = -num_neighbors; j <= num_neighbors; ++j) {
                // source
                r_y = o_y + j;
                r_y = (r_y > 0 ? (r_y < params.scaled_rows ? r_y : params.scaled_rows - 1) : 0) ;
                // reference
                r_ys = p.y + j;
                for (int i = -num_neighbors; i <= num_neighbors; ++i) {
                    // source
                    r_x = o_x + i;
                    r_x = (r_x > 0 ? (r_x < params.scaled_cols? r_x : params.scaled_cols - 1) : 0);
                    const int s_center = r_y*params.scaled_cols+r_x;
                    if (s_center >=  params.scaled_rows * params.scaled_cols) {
                        printf("Illegal: %d, %d, %f, %f (%d, %d)\n", r_x, r_y, o_x, o_y, params.scaled_cols,  params.scaled_rows);
                    }
                    srcPix = scaled_plane_hypotheses[s_center].w;
                    srcNorm = scaled_plane_hypotheses[s_center];
                    // refIm
                    r_xs = p.x + i;
                    neighborPix = tex2D<float>(texture_objects[0].images[0], r_xs + 0.5f, r_ys + 0.5f);

                    sgauss = SpatialGauss(o_x, o_y, r_x, r_y, sigmad);
                    rgauss = RangeGauss(fabs(refPix - neighborPix), sigmar);
                    totalgauss = sgauss * rgauss;
                    normalizing_factor += totalgauss;
                    c_total_val += srcPix * totalgauss;
                    mul4((&srcNorm), totalgauss);
                    n_total_val.x  = n_total_val.x + srcNorm.x;
                    n_total_val.y  = n_total_val.y + srcNorm.y;
                    n_total_val.z  = n_total_val.z + srcNorm.z;
                }
            }
            costs[center] = c_total_val / normalizing_factor;
            vecdiv4((&n_total_val), normalizing_factor);
            NormalizeVec3(&n_total_val);

             costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
            pre_costs[center] = costs[center];

            float4 plane_hypothesis = n_total_val;
            plane_hypothesis = TransformNormal2RefCam(cameras[0], plane_hypothesis);
            float depth = plane_hypotheses[center].w;
            plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depth, plane_hypothesis);
            plane_hypotheses[center] = plane_hypothesis;
            costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
         }
         else {
             float4 plane_hypothesis;
             if (params.hierarchy) {
                 plane_hypothesis = scaled_plane_hypotheses[center];
             }
             else {
                 plane_hypothesis = plane_hypotheses[center];
             }
             plane_hypothesis = TransformNormal2RefCam(cameras[0], plane_hypothesis);
             float depth = plane_hypothesis.w;
             plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depth, plane_hypothesis);
             plane_hypotheses[center] = plane_hypothesis;
             costs[center] = ComputeMultiViewInitialCostandSelectedViews(texture_objects[0].images, cameras, p, plane_hypotheses[center], &selected_views[center], params);
         }
    }
}

__device__ void PlaneHypothesisRefinement(const cudaTextureObject_t *images, const cudaTextureObject_t *depth_images, const Camera *cameras, float4 *plane_hypothesis, float *depth, float *cost, curandState *rand_state, const float *view_weights, const float weight_norm, float4 *prior_planes, unsigned int *plane_masks, float *restricted_cost, const int2 p, const PatchMatchParams params)
{
    float perturbation = 0.02f;
    const int center = p.y * cameras[0].width + p.x;

    float gamma = 0.5f;
    float depth_sigma = (params.depth_max - params.depth_min) / 64.0f;
    float two_depth_sigma_squared = 2 * depth_sigma * depth_sigma;
    float angle_sigma = M_PI * (5.0f / 180.0f);
    float two_angle_sigma_squared = 2 * angle_sigma * angle_sigma;
    float beta = 0.18f;
    float depth_prior = 0.0f;

    float depth_rand;
    float4 plane_hypothesis_rand;
    if (params.planar_prior && plane_masks[center] > 0) {
        depth_prior = ComputeDepthfromPlaneHypothesis(cameras[0], prior_planes[center], p);
        depth_rand = curand_uniform(rand_state) * 6 * depth_sigma + (depth_prior - 3 * depth_sigma);
        plane_hypothesis_rand = GeneratePerturbedNormal(cameras[0], p, prior_planes[center], rand_state, angle_sigma);
    }
    else {
        depth_rand = curand_uniform(rand_state) * (params.depth_max - params.depth_min) + params.depth_min;
        plane_hypothesis_rand = GenerateRandomNormal(cameras[0], p, rand_state, *depth);
    }
    float depth_perturbed = *depth;
    const float depth_min_perturbed = (1 - perturbation) * depth_perturbed;
    const float depth_max_perturbed = (1 + perturbation) * depth_perturbed;
    do {
        depth_perturbed = curand_uniform(rand_state) * (depth_max_perturbed - depth_min_perturbed) + depth_min_perturbed;
    } while (depth_perturbed < params.depth_min && depth_perturbed > params.depth_max);
    float4 plane_hypothesis_perturbed = GeneratePerturbedNormal(cameras[0], p, *plane_hypothesis, rand_state, perturbation * M_PI);

    const int num_planes = 5;
    float depths[num_planes] = {depth_rand, *depth, depth_rand, *depth, depth_perturbed};
    float4 normals[num_planes] = {*plane_hypothesis, plane_hypothesis_rand, plane_hypothesis_rand, plane_hypothesis_perturbed, *plane_hypothesis};

    for (int i = 0; i < num_planes; ++i) {
        float cost_vector[32] = {2.0f};
        float4 temp_plane_hypothesis = normals[i];
        temp_plane_hypothesis.w = GetDistance2Origin(cameras[0], p, depths[i], temp_plane_hypothesis);
        ComputeMultiViewCostVector(images, cameras, p, temp_plane_hypothesis, cost_vector, params);

        float temp_cost = 0.0f;
        for (int j = 0; j < params.num_images - 1; ++j) {
            if (view_weights[j] > 0) {
                if (params.geom_consistency) {
                    temp_cost += view_weights[j] * (cost_vector[j] + 0.2f * ComputeGeomConsistencyCost(depth_images[j+1], cameras[0], cameras[j+1], temp_plane_hypothesis, p));
                }
                else {
                    temp_cost += view_weights[j] * cost_vector[j];
                }
            }
        }
        temp_cost /= weight_norm;

        float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], temp_plane_hypothesis, p);
        if (params.planar_prior && plane_masks[center] > 0) {
            float depth_diff = depths[i] - depth_prior;
            float angle_cos = Vec3DotVec3(prior_planes[center], temp_plane_hypothesis);
            float angle_diff = acos(angle_cos);
            float prior = gamma + exp(- depth_diff * depth_diff / two_depth_sigma_squared) * exp(- angle_diff * angle_diff / two_angle_sigma_squared);
            float restricted_temp_cost = exp(-temp_cost * temp_cost / beta) * prior;
            if (depth_before >= params.depth_min && depth_before <= params.depth_max && restricted_temp_cost > *restricted_cost) {
                *depth = depth_before;
                *plane_hypothesis = temp_plane_hypothesis;
                *cost = temp_cost;
                *restricted_cost = restricted_temp_cost;
            }
        }
        else {
            if (depth_before >= params.depth_min && depth_before <= params.depth_max && temp_cost < *cost) {
                *depth = depth_before;
                *plane_hypothesis = temp_plane_hypothesis;
                *cost = temp_cost;
            }
        }
    }
}


//改
__device__ void ExtendedUpFarPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.x + p.y * width;
    int up_far = center - width * 5 - width * 2 * checkerboard_iter * 5;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.y > 4 + 2 * 5 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[up_far];
        costMinPoint = up_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.y > 4 + 2 * i + 2 * 5 * checkerboard_iter) {
                int pointTemp = up_far - 2 * i * width;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        if (costMin > costs[position[1]]) {
            costMin = costs[position[1]];
            costMinPoint = position[1];
            symbol = false;
        }
        up_far = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_far], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[1][i - 1];
            }
        }
    }
    else if (flag[1] == true) {
        up_far = position[1];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[1][i - 1];
        }
    }
    position[1] = up_far;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[1][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedDownFarPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int down_far = center + width * 5 + width * 2 * checkerboard_iter * 5;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.y < height - 5 - 2 * 5 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[down_far];
        costMinPoint = down_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.y < height - 5 - 2 * i - 2 * 5 * checkerboard_iter) {
                int pointTemp = down_far + 2 * i * width;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        if (costMin > costs[position[3]]) {
            costMin = costs[position[3]];
            costMinPoint = position[3];
            symbol = false;
        }
        down_far = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_far], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[3][i - 1];
            }
        }
    }
    else if (flag[3] == true) {
        down_far = position[3];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[3][i - 1];
        }
    }
    position[3] = down_far;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[3][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedLeftFarPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.x + p.y * width;
    int left_far = center - 5 - 5 * 2 * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x > 4 + 2 * 5 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[left_far];
        costMinPoint = left_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.x > 4 + 2 * i + 2 * 5 * checkerboard_iter) {
                int pointTemp = left_far - 2 * i;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        if (costMin > costs[position[5]]) {
            costMin = costs[position[5]];
            costMinPoint = position[5];
            symbol = false;
        }
        left_far = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_far], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[5][i - 1];
            }
        }
    }
    else if (flag[5] == true) {
        left_far = position[5];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[5][i - 1];
        }
    }
    position[5] = left_far;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[5][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedRightFarPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int right_far = center + 5 + 5 * 2 * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x < width - 5 - 2 * 5 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[right_far];
        costMinPoint = right_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.x < width - 5 - 2 * i - 2 * 5 * checkerboard_iter) {
                int pointTemp = right_far + 2 * i;
                if (costMin < costs[pointTemp]) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        if (costMin > costs[position[7]]) {
            costMin = costs[position[7]];
            costMinPoint = position[7];
            symbol = false;
        }
        right_far = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_far], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[7][i - 1];
            }
        }
    }
    else if (flag[7] == true) {
        right_far = position[7];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[7][i - 1];
        }
    }
    position[7] = right_far;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[7][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedLeftUpPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int left_up = center - 5 - 6 * width - 4 * 2 * checkerboard_iter - 4 * 2 * width * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x > 4 + 8 * checkerboard_iter && p.y > 5 + 8 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[left_up];
        costMinPoint = left_up;
        int position_x = p.x - 5 - 8 * checkerboard_iter;
        int position_y = p.y - 6 - 8 * checkerboard_iter;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_x = position_x - 2;
            }
            else
            {
                position_y = position_y - 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        if (costMin > costs[position[0]]) {
            costMin = costs[position[0]];
            costMinPoint = position[0];
            symbol = false;
        }
        left_up = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_up], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[0][i - 1];
            }
        }
    }
    else if (flag[0] == true) {
        left_up = position[0];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[0][i - 1];
        }
    }
    position[0] = left_up;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[0][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedRightUpPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int right_up = center + 6 - 5 * width + 4 * 2 * checkerboard_iter - 4 * 2 * width * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x < width - 6 - 8 * checkerboard_iter && p.y > 4 + 8 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[right_up];
        costMinPoint = right_up;
        int position_x = p.x + 6 + 8 * checkerboard_iter;
        int position_y = p.y - 5 - 8 * checkerboard_iter;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_y = position_y - 2;
            }
            else
            {
                position_x = position_x + 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        if (costMin > costs[position[2]]) {
            costMin = costs[position[2]];
            costMinPoint = position[2];
            symbol = false;
        }
        right_up = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_up], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[2][i - 1];
            }
        }
    }
    else if (flag[2] == true) {
        right_up = position[2];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[2][i - 1];
        }
    }
    position[2] = right_up;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[2][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedRightDownPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int right_down = center + 5 + 6 * width + 4 * 2 * checkerboard_iter + 4 * 2 * width * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x < width - 5 - 8 * checkerboard_iter && p.y < height - 6 - 8 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[right_down];
        costMinPoint = right_down;
        int position_x = p.x + 5 + 8 * checkerboard_iter;
        int position_y = p.y + 6 + 8 * checkerboard_iter;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_x = position_x + 2;
            }
            else
            {
                position_y = position_y + 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        if (costMin > costs[position[4]]) {
            costMin = costs[position[4]];
            costMinPoint = position[4];
            symbol = false;
        }
        right_down = costMinPoint;
        if (symbol) {
            ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_down], cost_array_tmp, params);
        }
        else {
            for (int i = 1; i < params.num_images; ++i) {
                cost_array_tmp[i - 1] = cost_array[4][i - 1];
            }
        }
    }
    else if (flag[4] == true) {
        right_down = position[4];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[4][i - 1];
        }
    }
    position[4] = right_down;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[4][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ void ExtendedLeftDownPropagation(const cudaTextureObject_t* images, const Camera* cameras, float4* plane_hypotheses, float* costs, unsigned int* selected_views, const int2 p, const PatchMatchParams params, int checkerboard_iter, int* position, float cost_array[][32], bool* flag) {
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }
    const int center = p.y * width + p.x;
    int left_down = center - 6 + 5 * width - 4 * 2 * checkerboard_iter + 4 * 2 * width * checkerboard_iter;
    float cost_array_tmp[32];
    float costMin;
    int costMinPoint;
    if (p.x > 5 + 8 * checkerboard_iter && p.y < height - 5 - 8 * checkerboard_iter) {
        bool symbol = true;
        costMin = costs[left_down];
        costMinPoint = left_down;
        int position_x = p.x - 6 - 8 * checkerboard_iter;
        int position_y = p.y + 5 + 8 * checkerboard_iter;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_y = position_y + 2;
            }
            else
            {
                position_x = position_x - 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            if (costMin > costs[position[6]]) {
                costMin = costs[position[6]];
                costMinPoint = position[6];
                symbol = false;
            }
            left_down = costMinPoint;
            if (symbol) {
                ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_down], cost_array_tmp, params);
            }
            else {
                for (int i = 1; i < params.num_images; ++i) {
                    cost_array_tmp[i - 1] = cost_array[6][i - 1];
                }
            }
        }
    }
    else if (flag[6] == true) {
        left_down = position[6];
        for (int i = 1; i < params.num_images; ++i) {
            cost_array_tmp[i - 1] = cost_array[6][i - 1];
        }
    }
    position[6] = left_down;
    for (int j = 1; j < params.num_images; j++) {
        cost_array[6][j - 1] = cost_array_tmp[j - 1];
    }
}

__device__ bool JudgeExtend(const int iter, int Extended_iter, int orientation, float(*cost_array)[32], const PatchMatchParams params, int flag) {
    if (!flag) {
        return false;
    }
    int iter_tmp = 3 - Extended_iter;
    float good_threshold = 0.8 * exp(-iter * iter * iter_tmp / 90.0);
    float bad_threshold = 1.2;
    int good_sum = 0;
    int bad_sum = 0;
    for (int i = 1; i < params.num_images; i++) {
        if (cost_array[orientation][i - 1] < good_threshold) {
            good_sum++;
        }
        if (cost_array[orientation][i - 1] > bad_threshold) {
            bad_sum++;
        }
    }
    if (good_sum >= 1 && bad_sum <= 2) {
        return false;
    }
    else {
        return true;
    }
}//改

__device__ void CheckerboardPropagation(const cudaTextureObject_t *images, const cudaTextureObject_t *depths, const Camera *cameras, float4 *plane_hypotheses, float *costs, float *pre_costs, curandState *rand_states, unsigned int *selected_views, float4 *prior_planes, unsigned int *plane_masks, const int2 p, const PatchMatchParams params, const int iter)
{
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }

    const int center = p.y * width + p.x;
    // int left_near = center - 1;
    // int left_far = center - 3;
    // int right_near = center + 1;
    // int right_far = center + 3;
    // int up_near = center - width;
    // int up_far = center - 3 * width;
    // int down_near = center + width;
    // int down_far = center + 3 * width;
//改
    //non-local strategy
    int left_up = center - 5 - 6 * width;
    int right_up = center + 6 - 5 * width;
    int right_down = center + 5 + 6 * width;
    int left_down = center - 6 + 5 * width;

    int left_far = center - 5;
    int right_far = center + 5;
    int up_far = center - width * 5;
    int down_far = center + width * 5;
    // Adaptive Checkerboard Sampling
    float cost_array[8][32] = {2.0f};
    // 0 -- up_near, 1 -- up_far, 2 -- down_near, 3 -- down_far, 4 -- left_near, 5 -- left_far, 6 -- right_near, 7 -- right_far
    bool flag[8] = {false};
    int num_valid_pixels = 0;

    float costMin;
    int costMinPoint;

    // // up_far
    // if (p.y > 2) {
    //     flag[1] = true;
    //     num_valid_pixels++;
    //     costMin = costs[up_far];
    //     costMinPoint = up_far;
    //     for (int i = 1; i < 11; ++i) {
    //         if (p.y > 2 + 2 * i) {
    //             int pointTemp = up_far - 2 * i * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     up_far = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_far], cost_array[1], params);
    // }

    // // dwon_far
    // if (p.y < height - 3) {
    //     flag[3] = true;
    //     num_valid_pixels++;
    //     costMin = costs[down_far];
    //     costMinPoint = down_far;
    //     for (int i = 1; i < 11; ++i) {
    //         if (p.y < height - 3 - 2 * i) {
    //             int pointTemp = down_far + 2 * i * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     down_far = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_far], cost_array[3], params);
    // }

    // // left_far
    // if (p.x > 2) {
    //     flag[5] = true;
    //     num_valid_pixels++;
    //     costMin = costs[left_far];
    //     costMinPoint = left_far;
    //     for (int i = 1; i < 11; ++i) {
    //         if (p.x > 2 + 2 * i) {
    //             int pointTemp = left_far - 2 * i;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     left_far = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_far], cost_array[5], params);
    // }

    // // right_far
    // if (p.x < width - 3) {
    //     flag[7] = true;
    //     num_valid_pixels++;
    //     costMin = costs[right_far];
    //     costMinPoint = right_far;
    //     for (int i = 1; i < 11; ++i) {
    //         if (p.x < width - 3 - 2 * i) {
    //             int pointTemp = right_far + 2 * i;
    //             if (costMin < costs[pointTemp]) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     right_far = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_far], cost_array[7], params);
    // }

    // // up_near
    // if (p.y > 0) {
    //     flag[0] = true;
    //     num_valid_pixels++;
    //     costMin = costs[up_near];
    //     costMinPoint = up_near;
    //     for (int i = 0; i < 3; ++i) {
    //         if (p.y > 1 + i && p.x > i) {
    //             int pointTemp = up_near - (1 + i) * width - (1 + i);
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //         if (p.y > 1 + i && p.x < width - 1 - i) {
    //             int pointTemp = up_near - (1 + i) * width + (1 + i);
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     up_near = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_near], cost_array[0], params);
    // }

    // // down_near
    // if (p.y < height - 1) {
    //     flag[2] = true;
    //     num_valid_pixels++;
    //     costMin = costs[down_near];
    //     costMinPoint = down_near;
    //     for (int i = 0; i < 3; ++i) {
    //         if (p.y < height - 2 - i && p.x > i) {
    //             int pointTemp = down_near + (1 + i) * width - (1 + i);
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //         if (p.y < height - 2 - i && p.x < width - 1 - i) {
    //             int pointTemp = down_near + (1 + i) * width + (1 + i);
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     down_near = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_near], cost_array[2], params);
    // }

    // // left_near
    // if (p.x > 0) {
    //     flag[4] = true;
    //     num_valid_pixels++;
    //     costMin = costs[left_near];
    //     costMinPoint = left_near;
    //     for (int i = 0; i < 3; ++i) {
    //         if (p.x > 1 + i && p.y > i) {
    //             int pointTemp = left_near - (1 + i) - (1 + i) * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //         if (p.x > 1 + i && p.y < height - 1 - i) {
    //             int pointTemp = left_near - (1 + i) + (1 + i) * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     left_near = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_near], cost_array[4], params);
    // }

    // // right_near
    // if (p.x < width - 1) {
    //     flag[6] = true;
    //     num_valid_pixels++;
    //     costMin = costs[right_near];
    //     costMinPoint = right_near;
    //     for (int i = 0; i < 3; ++i) {
    //         if (p.x < width - 2 - i && p.y > i) {
    //             int pointTemp = right_near + (1 + i) - (1 + i) * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //         if (p.x < width - 2 - i && p.y < height - 1- i) {
    //             int pointTemp = right_near + (1 + i) + (1 + i) * width;
    //             if (costs[pointTemp] < costMin) {
    //                 costMin = costs[pointTemp];
    //                 costMinPoint = pointTemp;
    //             }
    //         }
    //     }
    //     right_near = costMinPoint;
    //     ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_near], cost_array[6], params);
    // }
    // const int positions[8] = {up_near, up_far, down_near, down_far, left_near, left_far, right_near, right_far};
 //改
    // up_far
    if (p.y > 4) {
        flag[1] = true;
        num_valid_pixels++;
        costMin = costs[up_far];
        costMinPoint = up_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.y > 4 + 2 * i) {
                int pointTemp = up_far - 2 * i * width;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        up_far = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[up_far], cost_array[1], params);
    }

    // dwon_far
    if (p.y < height - 5) {
        flag[3] = true;
        num_valid_pixels++;
        costMin = costs[down_far];
        costMinPoint = down_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.y < height - 5 - 2 * i) {
                int pointTemp = down_far + 2 * i * width;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        down_far = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[down_far], cost_array[3], params);
    }

    // left_far
    if (p.x > 4) {
        flag[5] = true;
        num_valid_pixels++;
        costMin = costs[left_far];
        costMinPoint = left_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.x > 4 + 2 * i) {
                int pointTemp = left_far - 2 * i;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        left_far = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_far], cost_array[5], params);
    }

    // right_far
    if (p.x < width - 5) {
        flag[7] = true;
        num_valid_pixels++;
        costMin = costs[right_far];
        costMinPoint = right_far;
#pragma omp parallel for
        for (int i = 1; i < 5; ++i) {
            if (p.x < width - 5 - 2 * i) {
                int pointTemp = right_far + 2 * i;
                if (costMin < costs[pointTemp]) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else {
                break;
            }
        }
        right_far = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_far], cost_array[7], params);
    }

    //left_up
    if (p.x > 4 && p.y > 5) {
        flag[0] = true;
        num_valid_pixels++;
        costMin = costs[left_up];
        costMinPoint = left_up;
        int position_x = p.x - 5;
        int position_y = p.y - 6;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_x = position_x - 2;
            }
            else
            {
                position_y = position_y - 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        left_up = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_up], cost_array[0], params);
    }

    //right_up
    if (p.x < width - 6 && p.y>4) {
        flag[2] = true;
        num_valid_pixels++;
        costMin = costs[right_up];
        costMinPoint = right_up;
        int position_x = p.x + 6;
        int position_y = p.y - 5;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_y = position_y - 2;
            }
            else
            {
                position_x = position_x + 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        right_up = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_up], cost_array[2], params);
    }

    //right_down
    if (p.x < width - 5 && p.y < height - 6) {
        flag[4] = true;
        num_valid_pixels++;
        costMin = costs[right_down];
        costMinPoint = right_down;
        int position_x = p.x + 5;
        int position_y = p.y + 6;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_x = position_x + 2;
            }
            else
            {
                position_y = position_y + 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else
            {
                break;
            }
        }
        right_down = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[right_down], cost_array[4], params);
    }

    //left_down
    if (p.x > 5 && p.y < height - 5) {
        flag[6] = true;
        num_valid_pixels++;
        costMin = costs[left_down];
        costMinPoint = left_down;
        int position_x = p.x - 6;
        int position_y = p.y + 5;
#pragma omp parallel for
        for (int i = 0; i < 7; i++) {
            if (i % 2 == 0) {
                position_y = position_y + 2;
            }
            else
            {
                position_x = position_x - 2;
            }
            if (position_x >= 0 && position_x < width && position_y < height && position_y >= 0) {
                int pointTemp = position_y * width + position_x;
                if (costs[pointTemp] < costMin) {
                    costMin = costs[pointTemp];
                    costMinPoint = pointTemp;
                }
            }
            else 
            {
                break;
            }
        }
        left_down = costMinPoint;
        ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[left_down], cost_array[6], params);
    }
    int positions_tmp[8] = { left_up, up_far, right_up, down_far, right_down, left_far, left_down, right_far };

    //extensible strategy, extended eight orientations propagation
    int itertimes = 0;
    bool symbol_eight_orientations[10] = { true };
    while (itertimes < 3)
    {
        if (symbol_eight_orientations[0] == true) {
            if (JudgeExtend(iter, itertimes, 0, cost_array, params, flag[0])) {
                ExtendedLeftUpPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[0] = false;
            }
        }
        if (symbol_eight_orientations[1] == true) {
            if (JudgeExtend(iter, itertimes, 1, cost_array, params, flag[1])) {
                ExtendedUpFarPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[1] = false;
            }
        }
        if (symbol_eight_orientations[2] == true) {
            if (JudgeExtend(iter, itertimes, 2, cost_array, params, flag[2])) {
                ExtendedRightUpPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[2] = false;
            }
        }
        if (symbol_eight_orientations[3] == true) {
            if (JudgeExtend(iter, itertimes, 3, cost_array, params, flag[3])) {
                ExtendedDownFarPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[3] = false;
            }
        }
        if (symbol_eight_orientations[4] == true) {
            if (JudgeExtend(iter, itertimes, 4, cost_array, params, flag[4])) {
                ExtendedRightDownPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[4] = false;
            }
        }
        if (symbol_eight_orientations[5] == true) {
            if (JudgeExtend(iter, itertimes, 5, cost_array, params, flag[5])) {
                ExtendedLeftFarPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[5] = false;
            }
        }
        if (symbol_eight_orientations[6] == true) {
            if (JudgeExtend(iter, itertimes, 6, cost_array, params, flag[6])) {
                ExtendedLeftDownPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[6] = false;
            }
        }
        if (symbol_eight_orientations[7] == true) {
            if (JudgeExtend(iter, itertimes, 7, cost_array, params, flag[7])) {
                ExtendedRightFarPropagation(images, cameras, plane_hypotheses, costs, selected_views, p, params, itertimes, positions_tmp, cost_array, flag);
            }
            else {
                symbol_eight_orientations[7] = false;
            }
        }
        itertimes++;
    }

    const int positions[8] = { positions_tmp[0], positions_tmp[1] ,positions_tmp[2] ,positions_tmp[3] ,positions_tmp[4] ,positions_tmp[5] ,positions_tmp[6] ,positions_tmp[7] };//改

    // Multi-hypothesis Joint View Selection
    float view_weights[32] = {0.0f};
    float view_selection_priors[32] = {0.0f};
    int neighbor_positions[4] = { center - width * 5, center + width * 5, center - 5, center + 5 };//改
    // int neighbor_positions[4] = {center - width, center + width, center - 1, center + 1};
    for (int i = 0; i < 4; ++i) {
        if (flag[2 * i + 1]) {        //改
        // if (flag[2 * i]) {
            for (int j = 0; j < params.num_images - 1; ++j) {
                if (isSet(selected_views[neighbor_positions[i]], j) == 1) {
                    view_selection_priors[j] += 0.9f;
                } 
                else {
                    view_selection_priors[j] += 0.1f;
                }
            }
        }
    }

    float sampling_probs[32] = {0.0f};
    float cost_threshold = 0.8 * expf((iter) * (iter) / (-90.0f));
    for (int i = 0; i < params.num_images - 1; i++) {
        float count = 0;
        int count_false = 0;
        float tmpw = 0;
        for (int j = 0; j < 8; j++) {
            if (cost_array[j][i] < cost_threshold) {
                tmpw += expf(cost_array[j][i] * cost_array[j][i] / (-0.18f));
                count++;
            }
            if (cost_array[j][i] > 1.2f) {
                count_false++;
            }
        }
        if (count > 2 && count_false < 3) {
            sampling_probs[i] = tmpw / count;
        }
        else if (count_false < 3) {
            sampling_probs[i] = expf(cost_threshold * cost_threshold / (-0.32f));
        }
        sampling_probs[i] = sampling_probs[i] * view_selection_priors[i];
    }

    TransformPDFToCDF(sampling_probs, params.num_images - 1);
    for (int sample = 0; sample < 15; ++sample) {
        const float rand_prob = curand_uniform(&rand_states[center]) - FLT_EPSILON;

        for (int image_id = 0; image_id < params.num_images - 1; ++image_id) {
            const float prob = sampling_probs[image_id];
            if (prob > rand_prob) {
                view_weights[image_id] += 1.0f;
                break;
            }
        }
    }

    unsigned int temp_selected_views = 0;
    int num_selected_view = 0;
    float weight_norm = 0;
    for (int i = 0; i < params.num_images - 1; ++i) {
        if (view_weights[i] > 0) {
            setBit(temp_selected_views, i);
            weight_norm += view_weights[i];
            num_selected_view++;
        }
    }

    float final_costs[8] = {0.0f};
    for (int i = 0; i < 8; ++i) {
        for (int j = 0; j < params.num_images - 1; ++j) {
            if (view_weights[j] > 0) {
                if (params.geom_consistency) {
                    if (flag[i]) {
                        final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.1f * ComputeGeomConsistencyCost(depths[j + 1], cameras[0], cameras[j + 1], plane_hypotheses[positions[i]], p));//改
                        // final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.2f * ComputeGeomConsistencyCost(depths[j+1], cameras[0], cameras[j+1], plane_hypotheses[positions[i]], p));
                    }
                    else {
                        final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.1f * 5.0f);//改
                        // final_costs[i] += view_weights[j] * (cost_array[i][j] + 0.1f * 3.0f);
                    }
                }
                else {
                    final_costs[i] += view_weights[j] * cost_array[i][j];
                }
            }
        }
        final_costs[i] /= weight_norm;
    }

    const int min_cost_idx = FindMinCostIndex(final_costs, 8);

    float cost_vector_now[32] = {2.0f};
    ComputeMultiViewCostVector(images, cameras, p, plane_hypotheses[center], cost_vector_now, params);
    float cost_now = 0.0f;
    for (int i = 0; i < params.num_images - 1; ++i) {
        if (params.geom_consistency) {
            cost_now += view_weights[i] * (cost_vector_now[i] + 0.2f * ComputeGeomConsistencyCost(depths[i+1], cameras[0], cameras[i+1], plane_hypotheses[center], p));
        }
        else {
            cost_now += view_weights[i] * cost_vector_now[i];
        }
    }
    cost_now /= weight_norm;
    costs[center] = cost_now;
    float depth_now = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[center], p);
    float restricted_cost = 0.0f;
    if (params.planar_prior) {
        float restricted_final_costs[8] = {0.0f};
        float gamma = 0.5f;
        float depth_sigma = (params.depth_max - params.depth_min) / 64.0f;
        float two_depth_sigma_squared = 2 * depth_sigma * depth_sigma;
        float angle_sigma = M_PI * (5.0f / 180.0f);
        float two_angle_sigma_squared = 2 * angle_sigma * angle_sigma;
        float depth_prior = ComputeDepthfromPlaneHypothesis(cameras[0], prior_planes[center], p);
        float beta = 0.18f;

        if (plane_masks[center] > 0) {
            for (int i = 0; i < 8; i++) {
                if (flag[i]) {
                    float depth_now = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[positions[i]], p);
                    float depth_diff = depth_now - depth_prior;
                    float angle_cos = Vec3DotVec3(prior_planes[center], plane_hypotheses[positions[i]]);
                    float angle_diff = acos(angle_cos);
                    float prior = gamma + exp(- depth_diff * depth_diff / two_depth_sigma_squared) * exp(- angle_diff * angle_diff / two_angle_sigma_squared);
                    restricted_final_costs[i] = exp(-final_costs[i] * final_costs[i] / beta) * prior;
                }
            }
            const int max_cost_idx = FindMaxCostIndex(restricted_final_costs, 8);

            float restricted_cost_now = 0.0f;
            float depth_now = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[center], p);
            float depth_diff = depth_now - depth_prior;
            float angle_cos = Vec3DotVec3(prior_planes[center], plane_hypotheses[center]);
            float angle_diff = acos(angle_cos);
            float prior = gamma + exp(- depth_diff * depth_diff / two_depth_sigma_squared) * exp(- angle_diff * angle_diff / two_angle_sigma_squared);
            restricted_cost_now = exp(-cost_now * cost_now / beta) * prior;

            if (flag[max_cost_idx]) {
                float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[positions[max_cost_idx]], p);

                if (depth_before >= params.depth_min && depth_before <= params.depth_max && restricted_final_costs[max_cost_idx] > restricted_cost_now) {
                    depth_now   = depth_before;
                    plane_hypotheses[center] = plane_hypotheses[positions[max_cost_idx]];
                    costs[center] = final_costs[max_cost_idx];
                    restricted_cost = restricted_final_costs[max_cost_idx];
                    selected_views[center] = temp_selected_views;
                }
            }
        }
        else if (flag[min_cost_idx]) {
            float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[positions[min_cost_idx]], p);

            if (depth_before >= params.depth_min && depth_before <= params.depth_max && final_costs[min_cost_idx] < cost_now) {
                depth_now = depth_before;
                plane_hypotheses[center] = plane_hypotheses[positions[min_cost_idx]];
                costs[center] = final_costs[min_cost_idx];
            }
        }
    }

    float4 plane_hypotheses_now;
    if (!params.planar_prior && flag[min_cost_idx]) {
        float depth_before = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[positions[min_cost_idx]], p);

        if (depth_before >= params.depth_min && depth_before <= params.depth_max && final_costs[min_cost_idx] < cost_now) {
            depth_now = depth_before;
           plane_hypotheses_now = plane_hypotheses[positions[min_cost_idx]];
           cost_now = final_costs[min_cost_idx];
           selected_views[center] = temp_selected_views;
        }
    }

    PlaneHypothesisRefinement(images, depths, cameras, &plane_hypotheses_now, &depth_now, &cost_now, &rand_states[center], view_weights, weight_norm, prior_planes, plane_masks, &restricted_cost, p, params);

    if (params.hierarchy) {
        if (cost_now < pre_costs[center] - 0.1f) {
            costs[center] = cost_now;
            plane_hypotheses[center] = plane_hypotheses_now;
        }
    }
    else {
        costs[center] = cost_now;
        plane_hypotheses[center] = plane_hypotheses_now;
    }
}

__global__ void BlackPixelUpdate(cudaTextureObjects *texture_objects, cudaTextureObjects *texture_depths, Camera *cameras, float4 *plane_hypotheses, float *costs,  float *pre_costs,  curandState *rand_states, unsigned int *selected_views, float4 *prior_planes, unsigned int *plane_masks, const PatchMatchParams params, const int iter)
{
    int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    if (threadIdx.x % 2 == 0) {
        p.y = p.y * 2;
    } else {
        p.y = p.y * 2 + 1;
    }

    CheckerboardPropagation(texture_objects[0].images, texture_depths[0].images, cameras, plane_hypotheses, costs, pre_costs,  rand_states, selected_views, prior_planes, plane_masks, p, params, iter);
}

__global__ void RedPixelUpdate(cudaTextureObjects *texture_objects, cudaTextureObjects *texture_depths, Camera *cameras, float4 *plane_hypotheses, float *costs,  float *pre_costs, curandState *rand_states, unsigned int *selected_views, float4 *prior_planes, unsigned int *plane_masks, const PatchMatchParams params, const int iter)
{
    int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    if (threadIdx.x % 2 == 0) {
        p.y = p.y * 2 + 1;
    } else {
        p.y = p.y * 2;
    }

    CheckerboardPropagation(texture_objects[0].images, texture_depths[0].images, cameras, plane_hypotheses, costs, pre_costs, rand_states, selected_views, prior_planes, plane_masks, p, params, iter);
}

//从平面中计算深度法向量
__global__ void GetDepthandNormal(Camera *cameras, float4 *plane_hypotheses, const PatchMatchParams params)
{
    const int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    const int width = cameras[0].width;
    const int height = cameras[0].height;

    if (p.x >= width || p.y >= height) {
        return;
    }

    const int center = p.y * width + p.x;
    plane_hypotheses[center].w = ComputeDepthfromPlaneHypothesis(cameras[0], plane_hypotheses[center], p);
    plane_hypotheses[center] = TransformNormal(cameras[0], plane_hypotheses[center]);
}

//中值滤波
__device__ void CheckerboardFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs, const int2 p)
{
    int width = cameras[0].width;
    int height = cameras[0].height;
    if (p.x >= width || p.y >= height) {
        return;
    }

    const int center = p.y * width + p.x;

    float filter[21];
    int index = 0;

    filter[index++] = plane_hypotheses[center].w;

    // Left
    const int left = center - 1;
    const int leftleft = center - 3;

    // Up
    const int up = center - width;
    const int upup = center - 3 * width;

    // Down
    const int down = center + width;
    const int downdown = center + 3 * width;

    // Right
    const int right = center + 1;
    const int rightright = center + 3;

    if (costs[center] < 0.001f) {
        return;
    }

//如果点在图像内，将点的平面信息插入容器filter中
    if (p.y>0) {
        filter[index++] = plane_hypotheses[up].w;
    }
    if (p.y>2) {
        filter[index++] = plane_hypotheses[upup].w;
    }
    if (p.y>4) {
        filter[index++] = plane_hypotheses[upup-width*2].w;
    }
    if (p.y<height-1) {
        filter[index++] = plane_hypotheses[down].w;
    }
    if (p.y<height-3) {
        filter[index++] = plane_hypotheses[downdown].w;
    }
    if (p.y<height-5) {
        filter[index++] = plane_hypotheses[downdown+width*2].w;
    }
    if (p.x>0) {
        filter[index++] = plane_hypotheses[left].w;
    }
    if (p.x>2) {
        filter[index++] = plane_hypotheses[leftleft].w;
    }
    if (p.x>4) {
        filter[index++] = plane_hypotheses[leftleft-2].w;
    }
    if (p.x<width-1) {
        filter[index++] = plane_hypotheses[right].w;
    }
    if (p.x<width-3) {
        filter[index++] = plane_hypotheses[rightright].w;
    }
    if (p.x<width-5) {
        filter[index++] = plane_hypotheses[rightright+2].w;
    }
    if (p.y>0 &&
        p.x<width-2) {
        filter[index++] = plane_hypotheses[up+2].w;
    }
    if (p.y< height-1 &&
        p.x<width-2) {
        filter[index++] = plane_hypotheses[down+2].w;
    }
    if (p.y>0 &&
        p.x>1)
    {
        filter[index++] = plane_hypotheses[up-2].w;
    }
    if (p.y<height-1 &&
        p.x>1) {
        filter[index++] = plane_hypotheses[down-2].w;
    }
    if (p.x>0 &&
        p.y>2)
    {
        filter[index++] = plane_hypotheses[left  - width*2].w;
    }
    if (p.x<width-1 &&
        p.y>2)
    {
        filter[index++] = plane_hypotheses[right - width*2].w;
    }
    if (p.x>0 &&
        p.y<height-2) {
        filter[index++] = plane_hypotheses[left  + width*2].w;
    }
    if (p.x<width-1 &&
        p.y<height-2) {
        filter[index++] = plane_hypotheses[right + width*2].w;
    }

    sort_small(filter,index);
    int median_index = index / 2;
    if (index % 2 == 0) {
        plane_hypotheses[center].w = (filter[median_index-1] + filter[median_index]) / 2;
    } else {
        plane_hypotheses[center].w = filter[median_index];
    }
}

__global__ void BlackPixelFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs)
{
    int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    if (threadIdx.x % 2 == 0) {
        p.y = p.y * 2;
    } else {
        p.y = p.y * 2 + 1;
    }

    CheckerboardFilter(cameras, plane_hypotheses, costs, p);
}

__global__ void RedPixelFilter(const Camera *cameras, float4 *plane_hypotheses, float *costs)
{
    int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y);
    if (threadIdx.x % 2 == 0) {
        p.y = p.y * 2 + 1;
    } else {
        p.y = p.y * 2;
    }

    CheckerboardFilter(cameras, plane_hypotheses, costs, p);
}

void ACMMP::RunPatchMatch()
{
    const int width = cameras[0].width;
    const int height = cameras[0].height;

    int BLOCK_W = 32;
    int BLOCK_H = (BLOCK_W / 2);

    dim3 grid_size_randinit;
    grid_size_randinit.x = (width + 16 - 1) / 16;
    grid_size_randinit.y= (height + 16 - 1) / 16;
    grid_size_randinit.z = 1;
    dim3 block_size_randinit;
    block_size_randinit.x = 16;
    block_size_randinit.y = 16;
    block_size_randinit.z = 1;

    dim3 grid_size_checkerboard;
    grid_size_checkerboard.x = (width + BLOCK_W - 1) / BLOCK_W;
    grid_size_checkerboard.y= ( (height / 2) + BLOCK_H - 1) / BLOCK_H;
    grid_size_checkerboard.z = 1;
    dim3 block_size_checkerboard;
    block_size_checkerboard.x = BLOCK_W;
    block_size_checkerboard.y = BLOCK_H;
    block_size_checkerboard.z = 1;

    int max_iterations = params.max_iterations;

    RandomInitialization<<<grid_size_randinit, block_size_randinit>>>(texture_objects_cuda, cameras_cuda, plane_hypotheses_cuda, scaled_plane_hypotheses_cuda, costs_cuda, pre_costs_cuda, rand_states_cuda, selected_views_cuda, prior_planes_cuda, plane_masks_cuda, params);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());

    for (int i = 0; i < max_iterations; ++i) {
        BlackPixelUpdate<<<grid_size_checkerboard, block_size_checkerboard>>>(texture_objects_cuda, texture_depths_cuda, cameras_cuda, plane_hypotheses_cuda, costs_cuda, pre_costs_cuda, rand_states_cuda, selected_views_cuda, prior_planes_cuda, plane_masks_cuda, params, i);
        CUDA_SAFE_CALL(cudaDeviceSynchronize());
        RedPixelUpdate<<<grid_size_checkerboard, block_size_checkerboard>>>(texture_objects_cuda, texture_depths_cuda, cameras_cuda, plane_hypotheses_cuda, costs_cuda, pre_costs_cuda,rand_states_cuda, selected_views_cuda, prior_planes_cuda, plane_masks_cuda, params, i);
        CUDA_SAFE_CALL(cudaDeviceSynchronize());
        printf("iteration: %d\n", i);
    }

    GetDepthandNormal<<<grid_size_randinit, block_size_randinit>>>(cameras_cuda, plane_hypotheses_cuda, params);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());

    BlackPixelFilter<<<grid_size_checkerboard, block_size_checkerboard>>>(cameras_cuda, plane_hypotheses_cuda, costs_cuda);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());
    RedPixelFilter<<<grid_size_checkerboard, block_size_checkerboard>>>(cameras_cuda, plane_hypotheses_cuda, costs_cuda);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());

    cudaMemcpy(plane_hypotheses_host, plane_hypotheses_cuda, sizeof(float4) * width * height, cudaMemcpyDeviceToHost);
    cudaMemcpy(costs_host, costs_cuda, sizeof(float) * width * height, cudaMemcpyDeviceToHost);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());
}

//计算边缘势函数
__device__ float EstimateEdgeMRF(const cudaTextureObject_t depths, const Camera cameras, const int2 p, const int2 p_neighbor)
{
    float ref_depth = tex2D<float>(depths, (int)p.x + 0.5f, (int)p.y + 0.5f);      //当前帧中深度

    if (ref_depth <= 0.0)               //当前帧深度为0则跳过
        return 0;
    //计算边缘势函数
    float tmpedge(0);
    float depth_neighbor = tex2D<float>(depths, (int)p_neighbor.x + 0.5f, (int)p_neighbor.y + 0.5f);

    if(depth_neighbor>0){
        tmpedge = exp(-fabs(ref_depth-depth_neighbor)/ref_depth);
    }
    return tmpedge;
}
//计算节点势函数
__device__ float EstimateNodeMRF(const float ref_depth, const Camera ref_camera, const cudaTextureObject_t src_depths, const Camera src_camera, const int2 p, const float ref_cost)
{
    float3 forward_point = Get3DPointonWorld_cu(p.x, p.y, ref_depth, ref_camera);       //当前帧上当前点转到世界坐标

    float2 src_pt;
    float src_d;
    ProjectonCamera_cu(forward_point, src_camera, src_pt, src_d);                   //投影到邻域帧
    float src_depth = tex2D<float>(src_depths,  (int)src_pt.x + 0.5f, (int)src_pt.y + 0.5f);     //取邻域帧上深度值

    if (src_depth == 0.0f) {    //邻域帧上深度值为0跳过
        return 0.1f;
    }
    float3 src_3D_pt = Get3DPointonWorld_cu(src_pt.x, src_pt.y, src_depth, src_camera);     //邻域帧上投影点转到世界坐标

    float2 reproject_point;
    float ref_d;
    ProjectonCamera_cu(src_3D_pt, ref_camera, reproject_point, ref_d);      //投影回当前帧上

    float diff_col = p.x - reproject_point.x;             //计算重投影误差(x方向)
    float diff_row = p.y - reproject_point.y;             //计算重投影误差(y方向)
    float geo_diff = sqrt(diff_col*diff_col+diff_row*diff_row);       //计算重投影平方误差
    float depth_diff = fabs(ref_d - ref_depth) / ref_depth;     //计算重投影的深度差
    
    float node = 0;
    float sigma_d = 0.05;   //canshu
    float sigma_g = 5.0;
    float sigma_c = 0.5;
    
    node = exp(-((ref_cost*ref_cost)/(sigma_c*sigma_c) + (depth_diff*depth_diff)/(sigma_d*sigma_d) + (geo_diff)/(sigma_g*sigma_g)));   
    
    return node;
}

__global__ void EstimateConf(cudaTextureObject_t *texture_depths, Camera *cameras, float *costs, const PatchMatchParams params, float *mrfs_cuda)
{
    const int2 p = make_int2(blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y); 
    int width = cameras[0].width;       //图像宽
    int height = cameras[0].height;     //图像高

    if (p.x >= width || p.y >= height) {    
        return;
    }
    const int center = p.y * width + p.x;
    
    float ref_depth = tex2D<float>(texture_depths[0], (int)p.x + 0.5f, (int)p.y + 0.5f);      //当前帧中深度
    float ref_cost = costs[center];

    if (ref_depth <= 0.0)   //当前帧深度为0则跳过
        return;

    //计算边缘势函数
    float edges[4];
    //左边缘势函数
    if(p.x-1>0){
        const int2 pl = make_int2(p.x-1, p.y); 
        edges[0] = EstimateEdgeMRF(texture_depths[0], cameras[0], p, pl);
    }
    //右边缘势函数  
    if(p.x+1<width){
        const int2 pr = make_int2(p.x+1, p.y); 
        edges[1] = EstimateEdgeMRF(texture_depths[0], cameras[0], p, pr);
    }
    //上边缘势函数    
    if(p.y-1>0){
        const int2 pu = make_int2(p.x, p.y-1); 
        edges[2] = EstimateEdgeMRF(texture_depths[0], cameras[0], p, pu);
    }
    //下边缘势函数    
    if(p.y+1<height){
        const int2 pd = make_int2(p.x, p.y+1); 
        edges[3] = EstimateEdgeMRF(texture_depths[0], cameras[0], p, pd);
    }
    //计算最终边缘势函数
    float tmpedge = 1;
    float finaledge = 0;
    for(int e=0;e<4;e++){
        if(edges[e]>0){
            tmpedge = tmpedge*edges[e];
            finaledge = tmpedge;
        }
    }
    //计算节点势函数
    int neighbor_num = params.num_images-1;
    float nodes[21];
    for(int j=0; j<neighbor_num; j++){  //遍历所有邻域帧
        nodes[j] = EstimateNodeMRF(ref_depth, cameras[0], texture_depths[j+1], cameras[j+1], p, ref_cost);
        if (nodes[j] != nodes[j])
            nodes[j] = 0;
    }
    //找出最大值作为最终节点势函数
    float maxtmp = 0;
    for(int n=0; n<neighbor_num; n++){
        if(nodes[n]>maxtmp){
            maxtmp = nodes[n];
        }
    }
    float finalnode = maxtmp;   //最终的节点势函数为最大的一个   
    float mrf = finaledge * finalnode;  //最终的置信度
    mrfs_cuda[center] = mrf;
}

void ACMMP::ConfidenceEstimation()
{
    const int width = cameras[0].width;         //图像宽
    const int height = cameras[0].height;       //图像高

    dim3 grid_size_randinit;        //随机初始化网格大小
    grid_size_randinit.x = (width + 16 - 1) / 16;   // (图像宽+15)/16       3200分辨率为201
    grid_size_randinit.y=(height + 16 - 1) / 16;    // (图像高+15)/16
    grid_size_randinit.z = 1;
    dim3 block_size_randinit;       //随机初始化块大小
    block_size_randinit.x = 16;
    block_size_randinit.y = 16;
    block_size_randinit.z = 1;
    
    if(!params.fill_confidence){
        EstimateConf<<<grid_size_randinit, block_size_randinit>>>(texture_estimatedepths_cuda, cameras_cuda, costs_cuda, params, mrfs_cuda);
        CUDA_SAFE_CALL(cudaDeviceSynchronize());
    }
    else{
        EstimateConf<<<grid_size_randinit, block_size_randinit>>>(texture_filldepths_cuda, cameras_cuda, costs_cuda, params, mrfs_cuda);
        CUDA_SAFE_CALL(cudaDeviceSynchronize());
    }

    cudaMemcpy(mrfs_host, mrfs_cuda, sizeof(float) * width * height, cudaMemcpyDeviceToHost);
    CUDA_SAFE_CALL(cudaDeviceSynchronize());
}

__global__ void JBU_cu(JBUParameters *jp, JBUTexObj *jt, float *depth)
{
    const int2 p = make_int2 ( blockIdx.x * blockDim.x + threadIdx.x, blockIdx.y * blockDim.y + threadIdx.y );
    const int rows = jp[0].height;
    const int cols = jp[0].width;
    const int center = p.y * cols + p.x;

    if (p.x >= cols) {
        return;
    }
    if (p.y >= rows) {
        return;
    }

    const float scale  = 1.0 * jp[0].s_width / jp[0].width;
    const float sigmad = 0.50;
    const float sigmar = 25.5;
    const int WinWidth = jp[0].Imagescale * jp[0].Imagescale + 1;
    int num_neighbors = WinWidth / 2;

    const float o_y = p.y * scale;
    const float o_x = p.x * scale;
    const float refPix = tex2D<float>(jt[0].imgs[0], p.x + 0.5f, p.y + 0.5f);
    int r_y = 0;
    int r_ys = 0;
    int r_x = 0;
    int r_xs = 0;
    float sgauss = 0.0, rgauss = 0.0, totalgauss = 0.0;
    float total_val = 0.0, normalizing_factor = 0.0;
    float  srcPix = 0, neighborPix = 0;

    for (int j = -num_neighbors; j <= num_neighbors; ++j) {
        // source
        r_y = o_y + j;
        r_y = (r_y > 0 ? (r_y < jp[0].s_height ? r_y :jp[0].s_height - 1) : 0) ;
        // reference
        r_ys = p.y + j;
        r_ys = (r_ys > 0 ? (r_ys < jp[0].height ? r_ys :jp[0].height - 1) : 0) ;
        for (int i = -num_neighbors; i <= num_neighbors; ++i) {
            // source
            r_x = o_x + i;
            r_x = (r_x > 0 ? (r_x < jp[0].s_width ? r_x : jp[0].s_width - 1) : 0);
           srcPix = tex2D<float>(jt[0].imgs[1], r_x + 0.5f, r_y + 0.5f);
            // refIm
            r_xs = p.x + i;
            r_xs = (r_xs > 0 ? (r_xs < jp[0].width ? r_xs :jp[0].width - 1) : 0) ;
            neighborPix = tex2D<float>(jt[0].imgs[0], r_xs + 0.5f, r_ys + 0.5f);

            sgauss = SpatialGauss(o_x, o_y, r_x, r_y, sigmad);
            rgauss = RangeGauss(fabs(refPix - neighborPix), sigmar);
            totalgauss = sgauss * rgauss;
            normalizing_factor += totalgauss;
            total_val += srcPix * totalgauss;
        }
    }

    depth[center] = total_val / normalizing_factor;

}
void JBU::CudaRun()
{
    int rows = jp_h.height;
    int cols = jp_h.width;

    dim3 grid_size_initrand;
    grid_size_initrand.x= (cols + 16 - 1) / 16;
    grid_size_initrand.y=(rows + 16 - 1) / 16;
    grid_size_initrand.z= 1;
    dim3 block_size_initrand;
    block_size_initrand.x = 16;
    block_size_initrand.y = 16;
    block_size_initrand.z = 1;

    cudaEvent_t start, stop;
    cudaEventCreate(&start);
    cudaEventCreate(&stop);

    cudaEventRecord(start);

    cudaDeviceSynchronize();
    JBU_cu<<< grid_size_initrand, block_size_initrand>>>(jp_d, jt_d, depth_d);
    cudaDeviceSynchronize();

    cudaMemcpy(depth_h, depth_d, sizeof(float) * rows * cols, cudaMemcpyDeviceToHost);
    cudaDeviceSynchronize();

    cudaEventRecord(stop);
    cudaEventSynchronize(stop);
    float milliseconds = 0;
    cudaEventElapsedTime(&milliseconds, start, stop);
    printf("Total time needed for computation: %f seconds\n", milliseconds / 1000.f);
}
